#ifndef ARAL_INTERFACE_MODEL_HPP
#define ARAL_INTERFACE_MODEL_HPP

#include <string>
#include <vector>
#include <functional>
#include "aral/data_structure_definition.hpp"

namespace ARAL::interface {

/**
 * @class Model
 * @brief 定义机器人模型的静态物理属性和几何属性。
 *
 * @details
 *  `Model` 接口是机器人算法库的核心，它代表了一个机器人的“数字孪生”。
 *  这个类封装了所有不随时间变化的机器人固有属性，包括：
 *  - **运动学参数**: 连杆尺寸、关节类型、DH参数等。
 *  - **动力学参数**: 连杆质量、惯性张量、关节摩擦、负载信息等。
 *  - **几何参数**: 用于碰撞检测的3D模型（包括简化包围盒和精确几何体）。
 *
 *  `Model` 实例是构建更高级功能（如求解器 `Solver`、状态 `State`）的基础。
 *  它独立于机器人的瞬时状态（如当前关节角度或速度），专注于描述“这个机器人是什么”，而不是“它现在正在做什么”。
 */
class Model
{
protected:
    /**
     * @brief 虚析构函数。
     * @details 设置为 protected 可以防止用户意外地通过基类指针调用 delete，强制使用工厂返回的智能指针进行生命周期管理。
     */
    virtual ~Model() = default;

public:

 /**
     * @brief 设置机器人的功率限制值
     * @param joint_power_limits: 各关节最大运行功率
     * @param robot_power_limits: 整机最大耗散功率和供电功率
     * @return: 返回值 < 0, 表示设置失败
     */
    virtual int mdlSetRobotPowerLimits(const RLJntArray& joint_power_limits, const Array2d& robot_power_limits) = 0;

/**
     * @brief 获取机器人的功率限制值
     * @param joint_power_limits: 各关节最大运行功率
     * @param robot_power_limits: 整机最大耗散功率和供电功率
     * @return: 返回值 < 0, 表示获取失败
     */
    virtual int mdlGetRobotPowerLimits(RLJntArray& joint_power_limits, Array2d& robot_power_limits)const = 0;

    /**
     * @brief 获取机器人模型的名称。
     * @details 该名称通常与加载的URDF/SRDF文件中的机器人名称一致。
     * @return 机器人名称的常量引用。
     */
    virtual const std::string& mdlGetRobotName()const = 0;

    /**
     * @brief 获取机械臂的运动自由度（DOF）。
     * @details 这通常指的是机器人模型中非固定关节的总数。
     * @return 机器人自由度的无符号整数值。
     */
    virtual unsigned int mdlGetRobotDOF()const = 0;

    /**
     * @brief 设置机器人的DH参数误差值。
     * @details 该函数用于运动学标定后，将标定得到的DH参数误差（真实值 - 理论值）应用到模型中，以提高运动学精度。
     * @param error DH参数误差结构体的向量，每个结构体对应一个连杆。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetRobotDHParameterError(const std::vector<DHParameter>& error) = 0;

    /**
     * @brief 获取机械臂的DH参数。
     * @param realDH [in] 一个布尔值，用于选择获取理论参数还是实际参数。
     *               - `false`: 返回URDF/SRDF中定义的理论DH参数。
     *               - `true`: 返回叠加了 `mdlSetRobotDHParameterError` 所设误差后的实际DH参数。
     * @param dh_para [out] 用于存储DH参数的结构体向量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetRobotDHParameter(const bool realDH, std::vector<DHParameter>& dh_para)const = 0;

    /**
     * @brief 设置末端力传感器坐标系相对于机器人法兰坐标系的位姿。
     * @param pose 传感器坐标系的位姿（位置和RPY欧拉角）。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetEndSensorPoseInFlange(const RLPose& pose) = 0;

    /**
     * @brief 获取末端力传感器坐标系相对于机器人法兰坐标系的位姿。
     * @param pose [out] 用于存储传感器坐标系位姿的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetEndSensorPoseInFlange(RLPose& pose) = 0;

    /**
     * @brief 启用或禁用外部（远端）工具模式。
     * @details
     *   在标准模式下，工具(TCP)是安装在机器人法兰上的。
     *   在外部工具模式下，工具是环境中一个固定的点，或者由另一个机器人持有，机器人需要移动末端法兰去适配该工具。
     *   切换此模式会改变 `mdlSetToolPose` 和 `mdlSetWorkpiecePose` 的行为。
     * @warning 必须在设置工具和工件位姿之前调用此函数。
     * @param enable `true` 表示启用外部工具模式, `false` 表示禁用。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlEnableRemoteTool(const bool enable) = 0;

    /**
     * @brief 设置工具坐标系(TCP)的位姿。
     * @details
     *  - **标准模式**: TCP位姿是**相对于法兰坐标系**的。
     *  - **外部工具模式**: TCP位姿是**相对于基坐标系**的。
     * @see mdlEnableRemoteTool
     * @param pose TCP的位姿（位置和RPY欧拉角）。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetToolPose(const RLPose& pose) = 0;

    /**
     * @brief 获取工具坐标系(TCP)的位姿。
     * @details
     *  - **标准模式**: 返回TCP相对于法兰坐标系的位姿。
     *  - **外部工具模式**: 返回TCP相对于基坐标系的位姿。
     * @see mdlEnableRemoteTool
     * @param pose [out] 用于存储TCP位姿的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetToolPose(RLPose& pose) = 0;

    /**
     * @brief 设置工件坐标系的位姿。
     * @details
     *  - **标准模式**: 工件坐标系是**相对于基坐标系**的。
     *  - **外部工具模式**: 工件坐标系是**相对于法兰坐标系**的。
     * @see mdlEnableRemoteTool
     * @param pose 工件的位姿（位置和RPY欧拉角）。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetWorkpiecePose(const RLPose& pose) = 0;

    /**
     * @brief 获取工件坐标系的位姿。
     * @details
     *  - **标准模式**: 返回工件坐标系相对于基坐标系的位姿。
     *  - **外部工具模式**: 返回工件坐标系相对于法兰坐标系的位姿。
     * @see mdlEnableRemoteTool
     * @param pose [out] 用于存储工件位姿的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetWorkpiecePose(RLPose& pose) = 0;

    /**
     * @brief 设置重力加速度向量在机器人基坐标系下的描述。
     * @details
     *   重力是影响机器人动力学计算的重要环境因素。默认值通常为 {0, 0, -9.81}。
     *   如果机器人倾斜安装，则需要根据安装角度正确设置此向量。
     * @param vec 一个包含3个元素（X, Y, Z分量）的数组。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetGravityVectorInBase(const Array3d& vec) = 0;

    /**
     * @brief 获取重力加速度向量在机器人基坐标系下的描述。
     * @return 重力加速度向量的常量引用。
     */
    virtual const Array3d mdlGetGravityVectorInBase()const = 0;

    /**
     * @brief 设置机器人各关节的力矩常数(Torque Constant)。
     * @details 该参数定义了关节电机电流与输出力矩之间的关系，单位通常为 Nm/A。
     * @param constants 包含每个关节力矩常数的数组。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetJointTorqueConstant(const RLJntArray& constants) = 0;

    /**
     * @brief 获取机器人各关节的力矩常数。
     * @return 包含每个关节力矩常数的数组的常量引用。
     */
    virtual const RLJntArray mdlGetJointTorqueConstant()const = 0;

    /**
     * @brief 设置机器人各关节的转子惯量。
     * @param I_a 包含每个关节转子惯量的数组。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetJointRotorInertia(const RLJntArray& I_a) = 0;

    /**
     * @brief 获取机器人各关节的转子惯量。
     * @return 包含每个关节转子惯量的数组的常量引用。
     */
    virtual const RLJntArray mdlGetJointRotorInertia()const = 0;

    /**
     * @brief 设置关节摩擦力参数。
     * @details
     *   精确的摩擦模型对于高精度力控和轨迹跟踪至关重要。
     *   当前支持VTL（速度-温度-负载）模型。
     * @param friPara 摩擦力参数结构体的向量，每个关节一个。
     *   - **类型**: `VTL_Model`
     *   - **参数顺序**: `[Fs, Fc, Vs, Miu, Fv0, Fv1, Fv2, Fv3, Ft1, Ft2, Ft3, c1, c2]`
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;

    /**
     * @brief 获取关节摩擦力参数。
     * @param friPara [out] 用于存储摩擦力参数结构体的向量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetJointFrictionParameter(std::vector<FrictionParam>& friPara) = 0;

    /**
      * @brief 设置机器人的连杆集动力学参数。
      * @details 该函数用于一次性批量设置所有连杆的动力学参数，通常来自标定结果。
      * @param real_para 包含参数类型和具体数值的向量。
      *   - `real_para[0] == 0`: 包含60个参数 (10个/连杆 * 6连杆)，格式为 `[m, mx, my, mz, Ixx, Ixy, Ixz, Iyy, Iyz, Izz]`。
      *   - `real_para[0] == 1`: 包含58个参数，对应ARAL动力学标定接口的输出格式。
      *   - `real_para[0] == 2`: 包含42个参数，为旧版格式。
      * @return 成功返回0, 如果参数类型或大小不正确则返回错误码。
      */
    virtual int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para) = 0;

    /**
     * @brief 获取机器人的连杆集动力学参数。
     * @return 包含所有连杆动力学参数的向量，格式为 `[0, m1, mx1, ..., Izz1, ..., m6, ..., Izz6]`。
     */
    virtual std::vector<double> mdlGetRobotLinkDynamicParameter() = 0;

    /**
     * @brief 设置机器人指定单个连杆的动力学参数。
     * @param seg_name 要设置的连杆名称。
     * @param inertia 该连杆的动力学参数（质量、质心、惯性张量）。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetRobotSegmentDynamicParameter(const std::string& seg_name, const RLInertia& inertia) = 0;

    /**
      * @brief 获取机器人指定单个连杆的动力学参数。
      * @param seg_name 要获取的连杆名称。
      * @param inertia [out] 用于存储该连杆动力学参数的变量。
      * @return 成功返回0, 失败返回错误码。
      */
    virtual int mdlGetRobotSegmentDynamicParameter(const std::string& seg_name, RLInertia& inertia) = 0;

    /**
     * @brief 设置末端传感器的动力学参数。
     * @note 参数在末端**法兰坐标系**下定义。
     * @param inertia 传感器的动力学参数。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetEndSensorDynamicParameterInFlange(const RLInertia& inertia) = 0;

    /**
     * @brief 获取末端传感器的动力学参数。
     * @note 参数在末端**法兰坐标系**下定义。
     * @param inertia [out] 用于存储传感器动力学参数的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetEndSensorDynamicParameterInFlange(RLInertia& inertia) = 0;

    /**
     * @brief 设置负载（Payload）的动力学参数。
     * @note 参数在末端**法兰坐标系**下定义。负载通常指机器人抓取的工件。
     * @param inertia 负载的动力学参数。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia) = 0;

    /**
     * @brief 获取负载的动力学参数。
     * @note 参数在末端**法兰坐标系**下定义。
     * @param inertia [out] 用于存储负载动力学参数的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetLoadDynamicParameterInFlange(RLInertia& inertia) = 0;

    /**
     * @brief 获取机器人指定连杆的质量。
     * @param link_name 要查询的连杆名称。
     * @param link_mass [out] 用于存储连杆质量的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetLinkMass(const std::string& link_name, double& link_mass)const = 0;

    /**
     * @brief 获取机器人所有连杆的名称列表。
     * @return 包含所有连杆名称的字符串向量的常量引用。
     */
    virtual const std::vector<std::string>& mdlGetLinkName()const = 0;

    /**
     * @brief 获取指定连杆的OBB（Oriented Bounding Box）碰撞模型。
     * @details OBB是一种简化的矩形包围盒，用于快速、近似的碰撞检测。
     * @param link_name 要查询的连杆名称。
     * @return 该连杆对应的OBB模型向量（一个连杆可能由多个OBB组成）。
     */
    virtual std::vector<OBBPairs> mdlGetLinkOBBModel(const std::string& link_name)const = 0;

    /**
     * @brief 获取在特定关节构型下，所有连杆的OBB模型。
     * @details 此函数会首先通过正向运动学计算每个连杆的位姿，然后返回它们在基坐标系下的OBB模型。
     * @param q_in 机器人关节构型（关节角度）。
     * @param real_fk 是否使用实际DH参数进行正向运动学计算。
     * @return 一个从连杆名称到其OBB模型向量的映射。
     */
    virtual std::map<std::string, std::vector<OBBPairs>> mdlGetLinkOBBModels(const RLJntArray& q_in, const bool real_fk)const = 0;

    /**
     * @brief 为指定连杆设置精确的碰撞几何模型。
     * @details 除了OBB，还可以为连杆设置更精确的几何模型（如凸包、球体、圆柱体等）以提高碰撞检测的准确性。
     * @param link_name 要设置的连杆名称。
     * @param shapes 几何模型对象的智能指针向量。
     * @param origins 描述每个几何模型相对于其所属连杆坐标系的位姿。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetLinkCollisionGeometryModel(const std::string& link_name, const std::vector<geometric_shapes::ShapePtr>& shapes, const std::vector<RLPose>& origins)const = 0;

    /**
     * @brief 卸载（移除）指定连杆上已设置的某个碰撞几何模型。
     * @param link_name 目标连杆的名称。
     * @param shape_name 要卸载的几何模型的名称。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlUnsetLinkCollisionGeometryModel(const std::string& link_name, const std::vector<std::string>& shape_name)const = 0;

    /**
     * @brief 获取指定连杆上设置的精确碰撞几何模型。
     * @param link_name 要查询的连杆名称。
     * @param shapes [out] 用于存储几何模型对象智能指针的向量。
     * @param origins [out] 用于存储每个几何模型位姿的向量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetLinkCollisionGeometryModel(const std::string& link_name, std::vector<geometric_shapes::ShapeConstPtr>& shapes, std::vector<RLPose>& origins)const = 0;


    /**
     * @brief 设置机器人基坐标系(Base)在世界坐标系(World)下的位姿。
     * @details 这在多机器人场景或机器人安装在移动基座上时非常重要。
     * @param origin 基坐标系相对于世界坐标系的位姿。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetBasePoseInWorld(const RLPose& origin)const = 0;

    /**
     * @brief 获取机器人基坐标系在世界坐标系下的位姿。
     * @return 基坐标系位姿的常量引用。
     */
    virtual const RLPose mdlGetBasePoseInWorld()const = 0;

    /**
     * @brief [耗时接口] 计算并生成一个碰撞矩阵，用于加速后续的碰撞检测。
     * @details
     *   该函数通过在机器人工作空间内大量随机采样，来确定哪些连杆对“从不碰撞”或“总是碰撞”，
     *   从而在后续的碰撞检测中可以跳过这些连杆对，提高计算效率。
     * @param num_trials 随机采样构型的数量。数值越大，结果越可靠，但计算时间越长。
     * @param min_collision_fraction 判断“从不碰撞”的阈值比例。
     * @param verbose 是否打印详细的日志信息并生成SRDF文件。
     * @param map [out] 生成的邻接列表（碰撞矩阵），描述了连杆对之间的碰撞关系。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlComputeNeverInCollisionLinkPairs(const unsigned int num_trials, const double min_collision_fraction, const bool verbose, LinkPairMap& map) = 0;

    /**
     * @brief 设置机器人各关节的软件位置限位。
     * @note 这通常是比物理硬件限位更窄的一个安全范围。
     * @param upperLimit 各关节的位置上限。
     * @param lowerLimit 各关节的位置下限。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetJointPositionRange(const RLJntArray& upperLimit, const RLJntArray& lowerLimit) = 0;

    /**
     * @brief 获取机器人各关节的软件位置限位。
     * @param upperLimit [out] 用于存储各关节位置上限的变量。
     * @param lowerLimit [out] 用于存储各关节位置下限的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetJointPositionRange(RLJntArray& upperLimit, RLJntArray& lowerLimit) = 0;

    /**
     * @brief 获取机器人各关节的物理最大速度。
     * @details 该值通常在URDF模型中定义，代表了关节的物理性能极限。
     * @return 包含各关节最大速度的数组的常量引用。
     */
    virtual const RLJntArray mdlGetJointMaximumVelocity() = 0;

    /**
     * @brief 获取机器人各关节的最大平均负载力矩。
     * @param maxAverageTorque [out] 用于存储各关节最大平均负载力矩的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetJointMaximumAverageTorque(RLJntArray& maxAverageTorque) = 0;

    /**
     * @brief 获取机器人各关节的起停力矩。
     * @param startStopTorque [out] 用于存储各关节起停力矩的变量。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlGetJointStartStopTorque(RLJntArray& startStopTorque) = 0;

    /**
     * @brief 设置机器人各关节的起停力矩。
     * @note 该力矩值通常用于紧急停止功能。
     * @param startStopTorque 各关节的起停力矩。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int mdlSetJointStartStopTorque(const RLJntArray& startStopTorque) = 0;

};//class Model
typedef std::shared_ptr<Model>    ModelPtr;

}   //namespace ARAL::interface


#endif // ARAL_INTERFACE_MODEL_HPP